#!/bin/bash

# echo "安装所需依赖包..."

# sudo apt-get upgrade
# sudo apt-get install python3-pip
# pip3 install pyserial
# pip3 install Cython
# pip3 install setuptools
# pip3 install wheel
# pip3 install PyYAML==6.0
# sudo -H pip3 install rospkg
set -e

countdown=5

echo "等待 $countdown 秒以确保串口设备释放或连接成功" 

# 查找所有 /dev/ttyACM* 设备路径

for ((i=$countdown; i>-1; i--));do
    DEVICE_PATHS=$(sudo ls /dev/ttyACM*)
    #echo "找到 $DEVICE_PATH 串口设备"
    echo "$i"
    sleep 1
done


# 检查是否找到了设备路径
if [ -z "$DEVICE_PATHS" ]; then
    echo "没有找到关于 /dev/ttyACM* 的设备，请检查CAN模块设备是否连接成功。"
    exit 1
fi

echo "找到/dev/ttyACM*串口设备"

# 设置权限
for DEVICE_PATH in $DEVICE_PATHS; do
    # sudo chmod +x **/*.py
    echo "设置 $DEVICE_PATH 串口设备权限"
    sudo chmod 666 "$DEVICE_PATH"
done

# 捕获 Ctrl+C 信号，退出时自动清理后台 ROS 进程
cleanup() {
    echo ""
    echo "检测到中断信号，正在关闭所有 ROS 节点..."
    pkill -f "roslaunch"
    pkill -f "rosrun"
    echo "已安全退出"
    exit 0
}
trap cleanup SIGINT

# 启动 ROS 节点
roslaunch DrEmpower_can DrEmpower.launch device_paths:="$DEVICE_PATHS" &
sleep 2
rosrun joy joy_node &
sleep 2
roslaunch robot_arm_moveit_config real_remote.launch &
# 等待所有后台进程（阻塞脚本）
wait